#include "costmap_publish.h"


namespace costmap_2d{

Costmap2DPublisher::Costmap2DPublisher(Costmap2D* costmap, std::string global_frame, std::string topic_name):
    costmap_2d_(costmap), global_frame_(global_frame),cost_translation_table_(NULL){
    costmap_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>(topic_name, 1);

    if(cost_translation_table_ == NULL){
        cost_translation_table_ = new char[256];
    
        // special values:
        cost_translation_table_[0] = 0;  // NO obstacle
        cost_translation_table_[253] = 99;  // INSCRIBED obstacle
        cost_translation_table_[254] = 100;  // LETHAL obstacle
        cost_translation_table_[255] = -1;  // UNKNOWN
    
        // regular cost values scale the range 1 to 252 (inclusive) to fit
        // into 1 to 98 (inclusive).
        for (int i = 1; i < 253; i++){
            cost_translation_table_[ i ] = char(1 + (97 * (i - 1)) / 251);
        }
    }
}

void Costmap2DPublisher::getOccupancyGrid(nav_msgs::OccupancyGrid& grid){
    boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_2d_->getMutex()));
    grid.header.frame_id = global_frame_;
    grid.header.stamp = ros::Time::now();
    grid.info.resolution = costmap_2d_->getResolution();;
    grid.info.width = costmap_2d_->getWidth();
    grid.info.height = costmap_2d_->getHeight();

    double wx, wy;
    costmap_2d_->mapToWorld(0, 0, wx, wy);
    grid.info.origin.position.x = wx - grid.info.resolution / 2;
    grid.info.origin.position.y = wy - grid.info.resolution / 2;
    grid.info.origin.position.z = 0.0;
    grid.info.origin.orientation.w = 1.0;

    grid.data.resize(grid.info.width * grid.info.height);
    unsigned char* data = costmap_2d_->getCharMap();
    // cout << "data is null: " << (data == NULL) << ". size: " << grid.data.size() << endl;
    for(unsigned int i = 0; i < grid.data.size(); i++){
        // cout << "i: " << i << endl;
        // cout << "data: " << int(data[i]) << endl;
        grid.data[i] = cost_translation_table_[data[i]];
    }
    // cout << "getOccupancyGrid over..." << endl;
}


void Costmap2DPublisher::publishCostmap(){
    if(costmap_pub_.getNumSubscribers() == 0){
        return;
    }    
    nav_msgs::OccupancyGrid grid;
    getOccupancyGrid(grid);
    costmap_pub_.publish(grid);
}


}   // namespace costmap_2d